
#include <stdint.h>
#include <stdio.h>

#include "jtag.h"
#include "riscv_debug.h"

#define _CSR_DPC    0x7b1
#define _CSR_DCSR   0x7b0

static int ir_reg_val = RISCV_DTM_JTAG_REG_IDCODE; // 记录ir的值 减少设置ir
//
static inline void riscv_dtm_tick(uint32_t _ticks)
{
    jtag_tap_idle(_ticks);
}

void riscv_dtm_idcode(uint32_t *_idcode)
{
    uint32_t _inval;
    uint32_t _outval;

    _inval = RISCV_DTM_JTAG_REG_IDCODE;
    _outval = 0;
    jtag_tap_shift_ir(&_outval, &_inval, irlen);

    _inval = 0xffffffff;
    *_idcode = 0;
    jtag_tap_shift_dr(_idcode, &_inval, 32);
}

void riscv_dtm_dtmcs(riscv_dtmcs_t *_dtmcs)
{
    uint32_t inval;
    uint32_t outval;

    if (ir_reg_val != RISCV_DTM_JTAG_REG_DTMCS)
    {
        ir_reg_val = RISCV_DTM_JTAG_REG_DTMCS;
        inval = RISCV_DTM_JTAG_REG_DTMCS;
        outval = 0;
        jtag_tap_shift_ir(&outval, &inval, irlen);
    }
    inval = 0xffffffff;
    _dtmcs->reg = 0;
    jtag_tap_shift_dr(&_dtmcs->reg, &inval, 32);
}

static void riscv_dtm_dtmcs_dmireset(void)
{
    uint32_t inval;
    uint32_t outval;

    if (ir_reg_val != RISCV_DTM_JTAG_REG_DTMCS)
    {
        ir_reg_val = RISCV_DTM_JTAG_REG_DTMCS;
        inval = RISCV_DTM_JTAG_REG_DTMCS;
        jtag_tap_shift_ir(&outval, &inval, irlen);
    }

    inval = RISCV_DTMCS_DMIRESET;
    jtag_tap_shift_dr(&outval, &inval, 32);

    riscv_dtm_tick(dtmcs.idle);
}

static void riscv_dtm_dtmcs_dmihardreset(void)
{
    uint32_t inval;
    uint32_t outval;

    if (ir_reg_val != RISCV_DTM_JTAG_REG_DTMCS)
    {
        ir_reg_val = RISCV_DTM_JTAG_REG_DTMCS;
        inval = RISCV_DTM_JTAG_REG_DTMCS;
        jtag_tap_shift_ir(&outval, &inval, irlen);
    }

    inval = RISCV_DTMCS_DMIHARDRESET;
    jtag_tap_shift_dr(&outval, &inval, 32);

    riscv_dtm_tick(dtmcs.idle);
}

static inline void riscv_dtm_dmi(uint32_t addr, uint32_t *data, uint32_t *op)
{
    uint32_t inval[2] = {0};
    uint32_t outval[2] = {0};

    if (ir_reg_val != RISCV_DTM_JTAG_REG_DMI)
    {
        ir_reg_val = RISCV_DTM_JTAG_REG_DMI;
        inval[0] = RISCV_DTM_JTAG_REG_DMI;
        jtag_tap_shift_ir(&outval[0], &inval[0], irlen);
    }

    inval[0] = (*data << 2) | (*op & 0x3);
    inval[1] = (*data >> 30) | (addr << 2);
    outval[0] = 0;
    outval[1] = 0;
    jtag_tap_shift_dr(outval, inval, 32 + 2 + dtmcs.abits);

    *op = outval[0] & 0x3;
    *data = (outval[0] >> 2) | (outval[1] << 30);

    riscv_dtm_tick(dtmcs.idle);
}

//
static inline int riscv_dmi_nop(uint32_t *_op)
{
    uint32_t op;
    uint32_t data;

    op = RISCV_DMI_OP_NOP;
    data = 0;
    riscv_dtm_dmi(0, &data, &op);
    *_op = op;
    return op;
}

static int riscv_dmi_read(uint32_t addr, uint32_t *data, uint32_t *result)
{
    uint32_t op;
    uint32_t data0;

    op = RISCV_DMI_OP_READ;
    data0 = 0;
    riscv_dtm_dmi(addr, &data0, &op);
    for (int i = 0; i < 6; i++)
    {
        op = RISCV_DMI_OP_NOP;
        data0 = 0;
        riscv_dtm_dmi(0, &data0, &op);
        if (op == RISCV_DMI_RESULT_BUSY)
        {
            riscv_dtm_tick(32); // Run-Test-Idle
        }
        else
        {
            break;
        }
    }

    if (op != RISCV_DMI_RESULT_DONE)
    {
        riscv_dtm_dtmcs_dmireset();
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }
    else
    {
        *data = data0;
    }

    if (result)
    {
        *result = op;
    }

    return 0;
}

static int riscv_dmi_write(uint32_t addr, uint32_t data, uint32_t *result)
{
    uint32_t op;
    uint32_t data0;

    op = RISCV_DMI_OP_WRITE;
    data0 = data;
    riscv_dtm_dmi(addr, &data0, &op);

    if (riscv_dmi_nop(&op) != RISCV_DMI_RESULT_DONE)
    {
        riscv_dtm_dtmcs_dmireset();
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    if (result)
    {
        *result = op;
    }

    return 0;
}

//////////////////////////////////////////////////////////////////////////////
int riscv_debug_halt(void)
{
    riscv_dmcontrol_t dmcontrol;
    uint32_t rst;

    dmcontrol.reg = 0;
    dmcontrol.haltreq = 1;
    dmcontrol.dmactive = 1;
    int stat = riscv_dmi_write(RISCV_DM_CONTROL, dmcontrol.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }
    return 0;
}

int riscv_debug_halt_check(uint32_t *_stat)
{
    riscv_dmstatus_t dmstatus;
    uint32_t rst;

    dmstatus.reg = 0;
    int stat = riscv_dmi_read(RISCV_DM_STATUS, &dmstatus.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    *_stat = dmstatus.allhalted;
    return 0;
}

int riscv_debug_resume(unsigned char *_pc, char _flag)
{
    riscv_dmcontrol_t dmcontrol;
    uint32_t rst;

    if (_flag)
    {
        uint32_t pc_reg = *(uint32_t *)_pc;
        if (riscv_debug_write_register(&pc_reg, CSR_DPC, 4))
        {
            return -1;
        }
    }

    dmcontrol.reg = 0;
    dmcontrol.resumereq = 1;
    dmcontrol.dmactive = 1;
    dmcontrol.clrresethaltreq = 1;
    riscv_dmi_write(RISCV_DM_CONTROL, dmcontrol.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    //riscv_dmstatus_t dmstatus;
    //dmstatus.reg = 0;
    //riscv_dmi_read(RISCV_DM_STATUS, &dmstatus.reg, &rst);
    //asm volatile("ebreak");

    return 0;
}

int riscv_debug_resume_check(uint32_t *_stat)
{
    riscv_dmstatus_t dmstatus;
    uint32_t rst;

    dmstatus.reg = 0;
    riscv_dmi_read(RISCV_DM_STATUS, &dmstatus.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    *_stat = dmstatus.allrunning;
    return 0;
}

//////////////////////////////////////////////////////////////////////////////
// gdb xml中的CSRs序号和实际的序号差65的样子
int riscv_debug_read_register(uint8_t *_reg, uint32_t _regno, uint32_t _regsz)
{
    riscv_command_access_register_t cmd_reg;
    uint32_t rst = 0;

    cmd_reg.reg = 0;
    if (_regno <= 31) cmd_reg.regno = _regno + 0x1000;              // GPRs
    else if (_regno == 32) cmd_reg.regno = _CSR_DPC;                // PC
    else if (_regno <= 68) cmd_reg.regno = _regno + 0x1000 - 1;     // FPRs
    else if (_regno <= (0x1000 + 68)) cmd_reg.regno = _regno - 65;  // CSRs
    else if (_regno == 4161) cmd_reg.regno = _CSR_DCSR;             // priv
    else {cmd_reg.regno = 0xffffffff; RISCV_DEBUG_LOG_ERR(); return -1;}

    if (!(_regsz == 4 || _regsz == 8 || _regsz == 16))
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    uint32_t _sz;
    if (_regsz == 4) _sz = 2;
    else if(_regsz == 8) _sz = 3;
    else if(_regsz == 16) _sz = 4;
    cmd_reg.cmdtype = RISCV_DM_CMD_REG;
    cmd_reg.aarsize = _sz; // 寄存器位数 2-32b 3-64b 4-128b
    cmd_reg.transfer = 1;
    cmd_reg.write = 0;
    riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, cmd_reg.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    riscv_abstractcs_t abstractcs;
    rst = 0;
    abstractcs.reg = 0;
    riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &abstractcs.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }
    if (abstractcs.busy != 0 || abstractcs.cmderr != 0)
    {
        RISCV_DEBUG_LOG_ERR2(abstractcs.cmderr);
        rst = 0;
        abstractcs.reg = 0;
        abstractcs.cmderr = 1;
        riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, abstractcs.reg, &rst); // 如果这里再失败 就复位DM模块吧
        //asm volatile("ebreak");
        return -1;
    }

    if (_regsz == 16)
    {
        riscv_dmi_read(RISCV_DM_DATA3, &((uint32_t *)_reg)[3], &rst);
        if (rst != RISCV_DMI_RESULT_DONE)
        {
            RISCV_DEBUG_LOG_ERR();
            return -1;
        }
        riscv_dmi_read(RISCV_DM_DATA2, &((uint32_t *)_reg)[2], &rst);
        if (rst != RISCV_DMI_RESULT_DONE)
        {
            RISCV_DEBUG_LOG_ERR();
            return -1;
        }
    }
    if (_regsz >= 8)
    {
        riscv_dmi_read(RISCV_DM_DATA1, &((uint32_t *)_reg)[1], &rst);
        if (rst != RISCV_DMI_RESULT_DONE)
        {
            RISCV_DEBUG_LOG_ERR();
            return -1;
        }
    }
    riscv_dmi_read(RISCV_DM_DATA0, &((uint32_t *)_reg)[0], &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    return 0;
}

int riscv_debug_write_register(uint8_t *_reg, uint32_t _regno, uint32_t _regsz)
{
    uint32_t rst = 0;

    if (!(_regsz == 4 || _regsz == 8 || _regsz == 16))
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    uint32_t dm_data_reg;
    uint32_t _sz;
    if (_regsz == 4) _sz = 1;
    else if(_regsz == 8) _sz = 2;
    else if(_regsz == 16) _sz = 4;
    for (int i = 0; i < _sz; i++)
    {
        rst = 0;
        if (i == 0) dm_data_reg = RISCV_DM_DATA0;
        else if(i == 1) dm_data_reg = RISCV_DM_DATA1;
        else if(i == 2) dm_data_reg = RISCV_DM_DATA2;
        else if(i == 3) dm_data_reg = RISCV_DM_DATA3;
        riscv_dmi_write(dm_data_reg, ((uint32_t *)_reg)[i], &rst);
        if (rst != RISCV_DMI_RESULT_DONE)
        {
            RISCV_DEBUG_LOG_ERR();
            return -1;
        }
    }

    riscv_command_access_register_t cmd_reg;
    cmd_reg.reg = 0;
    rst = 0;
    if (_regno <= 31) cmd_reg.regno = _regno + 0x1000;              // GPRs
    else if (_regno == 32) cmd_reg.regno = _CSR_DPC;                // PC
    else if (_regno <= 68) cmd_reg.regno = _regno + 0x1000 - 1;     // FPRs
    else if (_regno <= (0x1000 + 68)) cmd_reg.regno = _regno - 65;  // CSRs
    else if (_regno == 4161) cmd_reg.regno = _CSR_DCSR;             // priv
    else {cmd_reg.regno = 0xffffffff; RISCV_DEBUG_LOG_ERR(); return -1;}
    if (_regsz == 4) _sz = 2;
    else if(_regsz == 8) _sz = 3;
    else if(_regsz == 16) _sz = 4;
    cmd_reg.cmdtype = RISCV_DM_CMD_REG;
    cmd_reg.aarsize = _sz;
    cmd_reg.transfer = 1;
    cmd_reg.write = 1;
    riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, cmd_reg.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    riscv_abstractcs_t abstractcs;
    abstractcs.reg = 0;
    rst = 0;
    riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &abstractcs.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }
    if (abstractcs.busy != 0 || abstractcs.cmderr != 0)
    {
        rst = 0;
        abstractcs.reg = 0;
        abstractcs.cmderr = 1;
        riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, abstractcs.reg, &rst); // 这里再失败 那就毁灭吧
        RISCV_DEBUG_LOG_ERR();
        // asm volatile("ebreak");
        return -1;
    }

    return 0;
}

int riscv_debug_read_mem8(riscv_mem_t _memaddr, void *buf, uint32_t _bytes)
{
    uint32_t rst;
    riscv_dmi_write(RISCV_DM_DATA1, _memaddr, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_command_access_memory_t cmd_mem;
    cmd_mem.reg = 0;
    cmd_mem.cmdtype = RISCV_DM_CMD_MEM;
    cmd_mem.aamsize = 0; // 8bits
    cmd_mem.write = 0;
    riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, cmd_mem.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_abstractcs_t abstractcs;
    abstractcs.reg = 0;
    riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &abstractcs.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }
    if (abstractcs.busy != 0 || abstractcs.cmderr != 0)
    {
        RISCV_DEBUG_LOG_ERR2(abstractcs.cmderr);
        rst = 0;
        abstractcs.reg = 0;
        abstractcs.cmderr = 1;
        riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, abstractcs.reg, &rst); // 如果这里再失败 就复位DM模块吧
        return -1;
    }

    uint32_t _tmp = 0;
    riscv_dmi_read(RISCV_DM_DATA0, &_tmp, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    *(char *)buf = _tmp & 0xff;
    return 0;
}

int riscv_debug_read_mem32(riscv_mem_t _memaddr, riscv_mem_t *buf, uint32_t _words)
{
    riscv_abstractauto_t abstractauto;
    uint32_t rst1;
    abstractauto.reg = 1;
    riscv_dmi_write(RISCV_DM_ABSTRACT_AUTO, abstractauto.reg, &rst1);
    if (rst1 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    uint32_t rst2;
    riscv_dmi_write(RISCV_DM_DATA1, _memaddr, &rst2);
    if (rst2 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_command_access_memory_t cmd_mem;
    uint32_t rst3;
    cmd_mem.reg = 0;
    cmd_mem.cmdtype = RISCV_DM_CMD_MEM;
    cmd_mem.aamsize = 2;
    cmd_mem.aampostincrement = 1;
    cmd_mem.write = 0;
    riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, cmd_mem.reg, &rst3);
    if (rst3 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    for (int i = 0; i < _words; i++)
    {
        if (i == (_words - 1))
        {
            abstractauto.reg = 0;
            riscv_dmi_write(RISCV_DM_ABSTRACT_AUTO, abstractauto.reg, &rst1);
            if (rst1 != RISCV_DMI_RESULT_DONE)
            {
                return -1;
            }
        }

        riscv_abstractcs_t abstractcs;
        uint32_t rst5;
        abstractcs.reg = 0;
        riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &abstractcs.reg, &rst2);
        if (rst1 != RISCV_DMI_RESULT_DONE)
        {
            return -1;
        }
        if (abstractcs.busy != 0 || abstractcs.cmderr != 0)
        {
            RISCV_DEBUG_LOG_ERR2(abstractcs.cmderr);
            uint32_t rst = 0;
            abstractcs.reg = 0;
            abstractcs.cmderr = 1;
            riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, abstractcs.reg, &rst); // 如果这里再失败 就复位DM模块吧
            return -1;
        }

        uint32_t rst4;
        riscv_dmi_read(RISCV_DM_DATA0, &buf[i], &rst4);
        if (rst4 != RISCV_DMI_RESULT_DONE)
        {
            return -1;
        }
    }

    return 0;
}

int riscv_debug_read_mem(riscv_mem_t _memaddr, void *_buf, uint32_t _bytes)
{
    uint32_t head = 0;
    uint32_t bytes4 = 0;
    uint32_t tail = 0;

    head = 4 - (_memaddr % 4);
    bytes4 = (_bytes - head) / 4;
    tail = _bytes - bytes4 * 4 - head;

    RISCV_DEBUG_LOG_DBG("head = 0x%x, bytes4 = 0x%x, tail = 0x%x \n", head, bytes4, tail);

    if (head)
    {
        for (int i = 0; i < head; i++)
        {
            if (riscv_debug_read_mem8(_memaddr + i, &((char *)_buf)[i], 1))
                return -1;
        }
    }

    if (bytes4)
    {
        if (riscv_debug_read_mem32(_memaddr + head, (char *)_buf + head, bytes4))
            return -1;
    }

    if (tail)
    {
        for (int i = 0; i < tail; i++)
        {
            if (riscv_debug_read_mem8(_memaddr + i + head + bytes4 * 4, &((char *)_buf + head + bytes4 * 4)[i], 1))
                return -1;
        }
    }

    return 0;
}

int riscv_debug_write_mem8(riscv_mem_t _memaddr, void *buf, uint32_t _bytes)
{
    uint32_t rst;
    riscv_dmi_write(RISCV_DM_DATA1, _memaddr, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_dmi_write(RISCV_DM_DATA0, *(char *)buf, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_command_access_memory_t cmd_mem;
    cmd_mem.reg = 0;
    cmd_mem.cmdtype = RISCV_DM_CMD_MEM;
    cmd_mem.aamsize = 0; // 8bits
    cmd_mem.write = 1;
    riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, cmd_mem.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_abstractcs_t abstractcs;
    abstractcs.reg = 0;
    riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &abstractcs.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }
    if (abstractcs.busy != 0 || abstractcs.cmderr != 0)
    {
        RISCV_DEBUG_LOG_ERR2(abstractcs.cmderr);
        asm volatile("ebreak");
        rst = 0;
        abstractcs.reg = 0;
        abstractcs.cmderr = 1;
        riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, abstractcs.reg, &rst); // 如果这里再失败 就复位DM模块吧
        asm volatile("ebreak");
        return -1;
    }

    return 0;
} 

int riscv_debug_write_mem32(riscv_mem_t _memaddr, riscv_mem_t *buf, uint32_t _words)
{

    uint32_t rst1;
    riscv_dmi_write(RISCV_DM_DATA1, _memaddr, &rst1);
    if (rst1 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    uint32_t rst2;
    riscv_dmi_write(RISCV_DM_DATA0, buf[0], &rst2);
    if (rst2 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_command_access_memory_t cmd_mem;
    uint32_t rst3;
    cmd_mem.reg = 0;
    cmd_mem.cmdtype = RISCV_DM_CMD_MEM;
    cmd_mem.aamsize = 2;
    cmd_mem.aampostincrement = 1;
    cmd_mem.write = 1;
    riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, cmd_mem.reg, &rst3);
    if (rst3 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    riscv_abstractauto_t abstractauto;
    uint32_t rst4;
    abstractauto.reg = 1;
    riscv_dmi_write(RISCV_DM_ABSTRACT_AUTO, abstractauto.reg, &rst4);
    if (rst4 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    for (int i = 1; i < _words; i++)
    {
#if 1
        riscv_abstractcs_t abstractcs;
        uint32_t rst5;
        abstractcs.reg = 0;
        riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &abstractcs.reg, &rst2);
        if (rst1 != RISCV_DMI_RESULT_DONE)
        {
            return -1;
        }
        if (abstractcs.busy != 0 || abstractcs.cmderr != 0)
        {
            RISCV_DEBUG_LOG_ERR2(abstractcs.cmderr);
            rst5 = 0;
            abstractcs.reg = 0;
            abstractcs.cmderr = 1;
            riscv_dmi_write(RISCV_DM_ABSTRACT_CMD, abstractcs.reg, &rst5); // 如果这里再失败 就复位DM模块吧
            return -1;
        }
#endif
        riscv_dmi_write(RISCV_DM_DATA0, buf[i], &rst2);
        if (rst2 != RISCV_DMI_RESULT_DONE)
        {
            return -1;
        }
    }

    abstractauto.reg = 0;
    riscv_dmi_write(RISCV_DM_ABSTRACT_AUTO, abstractauto.reg, &rst4);
    if (rst4 != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    return 0;
}

int riscv_debug_write_mem(riscv_mem_t _memaddr, void *_buf, uint32_t _bytes)
{
    uint32_t head = 0;
    uint32_t bytes4 = 0;
    uint32_t tail = 0;

    head = 4 - (_memaddr % 4);
    bytes4 = (_bytes - head) / 4;
    tail = _bytes - bytes4 * 4 - head;

    // RISCV_DEBUG_LOG_DBG("_memaddr = 0x%x, head = 0x%x, bytes4 = 0x%x, tail = 0x%x \n", _memaddr, head, bytes4, tail);

    if (head)
    {
        for (int i = 0; i < head; i++)
        {
            if (riscv_debug_write_mem8(_memaddr + i, &((char *)_buf)[i], 1))
                return -1;
        }
    }

    if (bytes4)
    {
        if (riscv_debug_write_mem32(_memaddr + head, (char *)_buf + head, bytes4))
            return -1;
    }

    if (tail)
    {
        for (int i = 0; i < tail; i++)
        {
            if (riscv_debug_write_mem8(_memaddr + i + head + bytes4 * 4, &((char *)_buf + head + bytes4 * 4)[i], 1))
                return -1;
        }
    }

    return 0;
}

riscv_breakpiont_t breakpiont;
// 返回-1表示无可用硬件断点  其他值表示断点号
static int riscv_debug_get_bkt_info(void)
{
    uint32_t tdata1 = 0;
    for (int i = 0; i < breakpiont.hwnum; i++)
    {
        riscv_debug_write_register(&i, CSR_TSELECT, 4);
        riscv_debug_read_register(&tdata1, CSR_TDATA1, 4);
        RISCV_DEBUG_LOG_DBG("tdata1 = 0x%x \n", tdata1);
        if (!(tdata1 & 0x3f))
        {
            RISCV_DEBUG_LOG_DBG("tselect = 0x%x \n", i);
            return i;
        }
    }
    // asm volatile("ebreak");
    return -1;
}
static int riscv_debug_set_bkt_info(unsigned char *addr, uint32_t hwno, char point_type, char set_clr)
{
    riscv_csr_mcontrol_t tdata1;
    uint32_t tdata2 = *(uint32_t *)addr;

    tdata1.reg = 0;
    tdata1.type = 2;    // 地址 数据 匹配
    tdata1.dmode = 1;   // 
    tdata1.action = 1;  // 进入调试模式
    tdata1.m = 1;
    tdata1.s = 1;
    tdata1.u = 1;

    if (!set_clr)
    {
        tdata1.m = 0;
        tdata1.s = 0;
        tdata1.u = 0;
        tdata1.execute = 0;
        tdata1.store = 0;
        tdata1.load = 0;
        tdata2 = 0;
    }
    else
    {
        switch (point_type)
        {
        case 0:
        {
            tdata1.execute = 1;
            tdata1.store = 0;
            tdata1.load = 0;
            break;
        }
        case 1:
        {
            tdata1.execute = 0;
            tdata1.store = 1;
            tdata1.load = 0;
            break;
        }
        case 2:
        {
            tdata1.execute = 0;
            tdata1.store = 0;
            tdata1.load = 1;
            break;
        }
        case 3:
        {
            tdata1.execute = 0;
            tdata1.store = 1;
            tdata1.load = 1;
            break;
        }
        default:
            return -1;
        }
    }

    RISCV_DEBUG_LOG_DBG("execute = 0x%x store = 0x%x load = 0x%x \n", tdata1.execute, tdata1.store, tdata1.load);
    riscv_debug_write_register(&hwno, CSR_TSELECT, 4);
    riscv_debug_write_register(&tdata1.reg, CSR_TDATA1, 4);
    riscv_debug_write_register(&tdata2, CSR_TDATA2, 4);

    return 0;
}
// 设置一个标志 用于记录target是32位就读4字节地址 是64位就读8字节地址
int riscv_debug_breakpiont(unsigned char *addr, char point_type, char set_clr)
{
    uint32_t bkt_addr = *(uint32_t *)addr;
    RISCV_DEBUG_LOG_DBG("bkt_addr = 0x%x \n", bkt_addr);
    if (set_clr)
    {
        for (int i = 0; i < breakpiont.breaknum; i++)
        {
            if (breakpiont.info[i].addr32 == bkt_addr)
            {
                return 0; // 地址断点已经存在
            }
        }

        for (int j = 0; j < breakpiont.breaknum; j++)
        {
            RISCV_DEBUG_LOG_DBG("breaknum = 0x%x \n", j);
            if (!breakpiont.info[j].enable)
            {
                // 0 - 3表示需要硬件断点支持
                if (point_type == 0 || point_type == 1 || point_type == 2 || point_type == 3)
                {
                    int hwno = riscv_debug_get_bkt_info();
                    if (hwno == -1)
                    {
                        return -1;
                    }
                    else
                    {
                        breakpiont.info[j].addr32 = bkt_addr;
                        breakpiont.info[j].hwno = hwno;
                        breakpiont.info[j].type = point_type;
                        breakpiont.info[j].enable = 1;
                        if (riscv_debug_set_bkt_info(addr, hwno, point_type, 1))
                        {
                            //asm volatile("ebreak");
                            return -1;
                        }
                        else
                        {
                            //asm volatile("ebreak");
                            return 0;
                        }
                    }
                }
                else
                {   //asm volatile("ebreak");
                    // 暂不支持软件断点
                    return -1;
                }
            }
        }
        //asm volatile("ebreak");
        return -1;
    }
    else // clr
    {
        for (int j = 0; j < breakpiont.breaknum; j++)
        {
            if (breakpiont.info[j].addr32 == bkt_addr)
            {
                if (breakpiont.info[j].enable)
                {   // 0 - 3表示需要硬件断点支持
                    if (breakpiont.info[j].type == 0 ||
                        breakpiont.info[j].type == 1 ||
                        breakpiont.info[j].type == 2 ||
                        breakpiont.info[j].type == 3)
                    {
                        if (riscv_debug_set_bkt_info(&breakpiont.info[j].addr32,
                                                     breakpiont.info[j].hwno,
                                                     breakpiont.info[j].type,
                                                     0))
                        {
                            return -1;
                        }
                        else
                        {
                            breakpiont.info[j].addr32 = -1;
                            breakpiont.info[j].hwno = -1;
                            breakpiont.info[j].type = -1;
                            breakpiont.info[j].enable = 0;
                            return 0;
                        }
                    }
                    else
                    { // 暂不支持软件断点
                        return -1;
                    }
                }
            }
        }

        return 0; // 不存在这个断点
    }
}

int riscv_debug_reset(char _flag)
{
    riscv_dmcontrol_t dmcontrol;
    uint32_t rst;

    dmcontrol.reg = 0;
    dmcontrol.dmactive = 1;
    if (_flag)
    {
        dmcontrol.setresethaltreq = 1;
        dmcontrol.haltreq = 1;
    }
    else
    {
        dmcontrol.clrresethaltreq = 1;
        dmcontrol.resumereq = 1;
    }
    dmcontrol.hartreset = 1;
    dmcontrol.ndmreset = 1;
    riscv_dmi_write(RISCV_DM_CONTROL, dmcontrol.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    // 延时 让target 复位成功
    for (volatile int i = 0; i < 8192; i++)
    {
        asm volatile("nop");
        asm volatile("nop");
        asm volatile("nop");
        asm volatile("nop");
    }

    //riscv_dmi_read(RISCV_DM_CONTROL, &dmcontrol.reg, &rst);
    //asm volatile("ebreak");
    //riscv_dmstatus_t dmstatus;
    //dmstatus.reg = 0;
    //riscv_dmi_read(RISCV_DM_STATUS, &dmstatus.reg, &rst);
    //asm volatile("ebreak");

    dmcontrol.reg = 0;
    dmcontrol.dmactive = 1;
    if (_flag)
    {
        dmcontrol.setresethaltreq = 1;
        dmcontrol.haltreq = 1;
    }
    else
    {
        dmcontrol.clrresethaltreq = 1;
        dmcontrol.resumereq = 1;
    }
    riscv_dmi_write(RISCV_DM_CONTROL, dmcontrol.reg, &rst);
    if (rst != RISCV_DMI_RESULT_DONE)
    {
        return -1;
    }

    //riscv_dmi_read(RISCV_DM_CONTROL, &dmcontrol.reg, &rst);
    //asm volatile("ebreak");
    //dmstatus;
    //dmstatus.reg = 0;
    //riscv_dmi_read(RISCV_DM_STATUS, &dmstatus.reg, &rst);
    //asm volatile("ebreak");

    return 0;
}

int riscv_debug_halt_reason(void)
{
    uint32_t stat;
    uint32_t PC;
    if (!riscv_debug_halt_check(&stat))
    {   
        if (stat)
        {
            riscv_csr_dcsr_t dcsr;
            dcsr.reg = 0;
            if (!riscv_debug_read_register(&dcsr.reg, CSR_DCSR, 4))
            {
                if (dcsr.cause == 1) return 4; // ebreak swbreak
                if (dcsr.cause == 2) // 0-hwbreak 1-watchpoint(write) 2-watchpoint(read) 3-watchpoint(access)
                {
                    if (riscv_debug_read_register(&PC, CSR_DPC, 4))
                    {
                        return -1;
                    }
                    for (int i = 0; i < breakpiont.breaknum; i++)
                    {
                        if (breakpiont.info[i].addr32 == PC)
                        {
                            return breakpiont.info[i].type;
                        }
                    }
                }
                if (dcsr.cause == 3) return 5; // other
            }
        }
    }

    return -1;
}

int riscv_debug_step(char flag)
{
    riscv_csr_dcsr_t dcsr;
    uint32_t rst;

    if (riscv_debug_read_register(&dcsr.reg, CSR_DCSR, 4))
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }
    
    if (flag) dcsr.step = 1;
    else dcsr.step = 0;

    if (riscv_debug_write_register(&dcsr.reg, CSR_DCSR, 4))
    {
        RISCV_DEBUG_LOG_ERR();
        return -1;
    }

    return 0;
}

void riscv_debug_init(void)
{
    riscv_dmcontrol_t dmcontrol1, dmcontrol2, dmcontrol3, dmcontrol4;
    uint32_t rst1, rst2, rst3, rst4;
    jtag_pin_init();
    jtag_tap_init();
    jtag_irlen_tapnum();
// asm volatile("ebreak");
#if 1
    riscv_dtm_idcode(&idcode);
    riscv_dtm_dtmcs(&dtmcs);
    dtmcs.idle -= (dtmcs.idle ? 1 : 0); // shift ir/dr 最后都会进入idle状态 所以此处需要减1

    dmcontrol1.reg = 0;
    riscv_dmi_write(RISCV_DM_CONTROL, dmcontrol1.reg, &rst1);
    if (rst1 != RISCV_DMI_RESULT_DONE)
    {
    }
    dmcontrol2.reg = 0;
    riscv_dmi_read(RISCV_DM_CONTROL, &dmcontrol2.reg, &rst2);
    if (rst2 != RISCV_DMI_RESULT_DONE)
    {
    }
    if (dmcontrol2.dmactive != 0)
    {
    }
    //asm volatile("ebreak");
    dmcontrol3.reg = 0;
    dmcontrol3.dmactive = 1;
    riscv_dmi_write(RISCV_DM_CONTROL, dmcontrol3.reg, &rst3);
    if (rst3 != RISCV_DMI_RESULT_DONE)
    {
    }
    dmcontrol4.reg = 0;
    riscv_dmi_read(RISCV_DM_CONTROL, &dmcontrol4.reg, &rst4);
    if (rst4 != RISCV_DMI_RESULT_DONE)
    {
    }
    if (dmcontrol4.dmactive != 1)
    {
    }
    //asm volatile("ebreak");
    //
    riscv_dm.dmstatus.reg = 0;
    riscv_dmi_read(RISCV_DM_STATUS, &riscv_dm.dmstatus.reg, &rst1);
    if (rst1 != RISCV_DMI_RESULT_DONE)
    {
    }
    //asm volatile("ebreak");
    riscv_dm.abstractcs.reg = 0;
    riscv_dmi_read(RISCV_DM_ABSTRACT_CS, &riscv_dm.abstractcs.reg, &rst1);
    if (rst1 != RISCV_DMI_RESULT_DONE)
    {
    }
    //asm volatile("ebreak");
    riscv_dm.sbcs.reg = 0;
    riscv_dmi_read(RISCV_DM_SB_CS, &riscv_dm.sbcs.reg, &rst1);
    if (rst1 != RISCV_DMI_RESULT_DONE)
    {
    }
    //asm volatile("ebreak");
#endif
    
    uint32_t stat;
    riscv_debug_halt();
    riscv_debug_halt_check(&stat);
    if (!stat) return;

    riscv_csr_dcsr_t dcsr;
    riscv_debug_read_register(&dcsr.reg, CSR_DCSR, 4);
    //asm volatile("ebreak");
    dcsr.ebreakm = 1;
    dcsr.ebreaks = 1;
    dcsr.ebreaku = 1;
    riscv_debug_write_register(&dcsr.reg, CSR_DCSR, 4);
    //asm volatile("ebreak");

    riscv_csr_mcontrol_t tdata1;
    uint32_t tselect = 0;
    uint32_t tdata2 = 0;

    int i;
    for (i = 0; i < 32; i++)
    {
        riscv_debug_write_register((uint8_t *)&i, CSR_TSELECT, 4);
        riscv_debug_read_register(&tselect, CSR_TSELECT, 4);
        riscv_debug_read_register(&tdata1, CSR_TDATA1, 4);
        riscv_debug_read_register(&tdata2, CSR_TDATA2, 4);

        if (tselect != i)
        {
            break;
        }

        tdata1.reg = 0;
        tdata1.type = 2;    // 地址 数据 匹配
        tdata1.dmode = 1;   // 
        tdata1.action = 1;  // 进入调试模式
        tdata1.m = 0;
        tdata1.s = 0;
        tdata1.u = 0;
        tdata1.execute = 0;
        tdata1.store = 0;
        tdata1.load = 0;

        riscv_debug_write_register(&tselect, CSR_TSELECT, 4);
        riscv_debug_write_register(&tdata1.reg, CSR_TDATA1, 4);
        riscv_debug_write_register(&tdata2, CSR_TDATA2, 4);
    }
    breakpiont.hwnum = i;
    breakpiont.breaknum = 32;
    for (int j = 0; j < breakpiont.breaknum; j++)
    {
        breakpiont.info[j].addr32 = -1;
        breakpiont.info[j].enable = 0;
        breakpiont.info[j].hwno = -1;
        breakpiont.info[j].type = -1;
    }
}

static unsigned int crc8_d(const unsigned char * data, unsigned int length)
{
    const unsigned char *ptr = (const unsigned char *)data;
    volatile unsigned int len = length;
    volatile unsigned char crc = 0xa5;
    while (len--)
    {
        crc ^= *ptr++; // crc ^= *data; data++;
        for (volatile unsigned int i = 0; i < 8; i++)
        {
            if (crc & 0x80)
                crc = (crc << 1) ^ 0x07;
            else
                crc <<= 1;
        }
    }

    RISCV_DEBUG_LOG_DBG("crc8 %x \n", crc);
    return crc;
}

int riscv_debug_sysbus_write_mem(unsigned int adr, unsigned char *buf, unsigned int sz, unsigned int crc_flag)
{
    unsigned int head = 0;
    unsigned int bytes4 = 0;
    unsigned int tail = 0;
    unsigned int rst = 0;
    unsigned int op;
    unsigned int data;
    unsigned char *p_head_buf;
    unsigned int *p_bytes4_buf;
    unsigned char *p_tail_buf;

    if (adr % 4 != 0) 
        head = 4 - (adr % 4);
    bytes4 = (sz - head) / 4;
    tail = sz - bytes4 * 4 - head;

    RISCV_DEBUG_LOG_DBG("head = %d, bytes4 = %d, tail = %d \n", head, bytes4, tail);

    p_head_buf = (unsigned char *)buf;
    p_bytes4_buf = (unsigned int *)(buf + head);
    p_tail_buf = (unsigned char *)(buf + head + bytes4 * 4);

    riscv_sbcs_t sbcs;
    sbcs.reg = 0;
    riscv_dmi_read(RISCV_DM_SB_CS, &sbcs.reg, &rst);
    if (sbcs.sbbusy) return -1;

    if (head)
    {
        sbcs.reg = 0;
        sbcs.sbautoincrement = 1;
        sbcs.sbaccess = 0;
        riscv_dmi_write(RISCV_DM_SB_CS, sbcs.reg, &rst);
        riscv_dmi_write(RISCV_DM_SB_ADDR0, adr, &rst);

        for (volatile int i = 0; i < head; i++)
        {
            op = RISCV_DMI_OP_WRITE;
            data = *p_head_buf++;
            riscv_dtm_dmi(RISCV_DM_SB_DATA0, &data, &op);
        }
    }

    if (bytes4)
    {
        sbcs.reg = 0;
        sbcs.sbautoincrement = 1;
        sbcs.sbaccess = 2;
        riscv_dmi_write(RISCV_DM_SB_CS, sbcs.reg, &rst);
        riscv_dmi_write(RISCV_DM_SB_ADDR0, adr + head, &rst);

        for (volatile int i = 0; i < bytes4; i++)
        {
            op = RISCV_DMI_OP_WRITE;
            data = *p_bytes4_buf++;
            riscv_dtm_dmi(RISCV_DM_SB_DATA0, &data, &op);
        }
    }

    if (tail)
    {
        sbcs.reg = 0;
        sbcs.sbautoincrement = 1;
        sbcs.sbaccess = 0;
        riscv_dmi_write(RISCV_DM_SB_CS, sbcs.reg, &rst);
        riscv_dmi_write(RISCV_DM_SB_ADDR0, adr + head + bytes4 * 4, &rst);

        for (volatile int i = 0; i < tail; i++)
        {
            op = RISCV_DMI_OP_WRITE;
            data = *p_tail_buf++;
            riscv_dtm_dmi(RISCV_DM_SB_DATA0, &data, &op);
        }
    }

    extern int flash_crc8_d(unsigned int loadadr, unsigned int fun, unsigned int adr, unsigned int sz, unsigned int crc);

    if (crc_flag)
        if (!flash_crc8_d(((adr + sz) / 4 + 1) * 4 , 0x0c, adr, sz, crc8_d(buf, sz)))
            return -1;

    return 0;
}

riscv_dtmcs_t dtmcs;
riscv_dm_t riscv_dm;
